clc
clear all


syms t1 t2 t3 l1 l2 l3;


%test page 329

T01 = [cos(t1) 0 sin(t1) 0;
        sin(t1) 0 -cos(t1) 0;
        0 1 0 l1;
        0 0 0 1]
    
T12 = [cos(t2) -sin(t2) 0 l2*cos(t2);
        sin(t2) cos(t2) 0 l2*sin(t2);
        0 0 1 0;
        0 0 0 1]
    
T23 = [cos(t3) 0 sin(t3) 0; 
       sin(t3) 0 -cos(t3) 0;
       0 1 0 0;
       0 0 0 1]
   
 Tdp = [ 1 0 0 0;
         0 1 0 0;
         0 0 1 l3;
         0 0 0 1]
   
   
%Dp = [0; 0; l3; 1]

Tr_tmp = T01 * T12 * T23 * Tdp
Tr = simplify(Tr_tmp)

%D0p = Tr*Dp


iters = 50

l1 = 1
l2 = 1.05
l3 = 0.89
T_req = [1; 1.1; 1.2]      % Required XYZ coords


% Build symbolic matrices
T = Tr(1:3,4)            % Get the final position
J = jacobian(T, [t1 t2 t3])
J_inv = inv(J)

q = zeros(3,4)
q(:,1) = [0.8; 0.7; 0.1]      % Initial Guess
%q(:,1) = [pi/3; pi/3; pi/3]      % Initial Guess


for i=2:iters
    t1 = q(1, i-1)
    t2 = q(2, i-1)
    t3 = q(3, i-1)
    
    T_tmp = eval(T)                 % Eval the residue
    delta_T = T_req - T_tmp
    J_tmp = eval(J)                 % find the jacobian of it
    J_inv_tmp = eval(J_inv)         % find the inverse jacobian
    
    q(:,i) = q(:,i-1) + J_inv_tmp*delta_T       %Calculate the next iteration

end

    t1 = q(1, iters)
    t2 = q(2, iters)
    t3 = q(3, iters)
    
    eval(Tr(1:3,4))
    




